#include "libuvc/UVCCamera.hpp"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <math.h>
#include <stdlib.h>
#include <sys/time.h>
#include <filesystem>
#include <thread>
#include <chrono>
#include <wiringPi.h>
//#include <softPwm.h>
//#include <wiringSerial.h>
#include "serial_C++.hpp"
#include "AsyncVideoWriter.h"
#include "StateMachine.h"
Receive_Data_C* preceive_data = nullptr;
void callback(const std::vector<uint8_t>& data)
{
	if(preceive_data == nullptr)
	return;
	else
	{
		preceive_data->data_arrange(reinterpret_cast<const char*>(data.data()));
		//preceive_data->my_print();
		preceive_data->Data_Storage();
		return;
	}
    
    //receive_data_c.my_print();
	
}

double fx=338.5159453102525;
double fy=338.671849473212;

double duration = 30;
int debug = 1;
int show = 1;

int main(int argc, char* argv[])
{
	bool start_mode = false; //是否为开机脚本启动
	std::cout << "参数个数: " << argc << std::endl;
	if(argc > 2)
	{
		std::cout << "开机脚本启动" << std::endl;
		start_mode = true;
	}
	else if (argc < 2)
	{
		std::cout << "过少输入参数" << std::endl;
		return -1;
	}
	else if(argc == 2)
	{
		std::cout << "参数个数正确" << std::endl;
	}
	long long test_number = std::stoll(argv[1]);
	std::cout << "当前计数:" << test_number << std::endl;
	std::string filename = std::string("../fly_data") + argv[1] + ".txt";
	std::ofstream flydata_handle(filename);
	//开启串口	
	Receive_Data_C receive_data_c(flydata_handle);
	preceive_data = &receive_data_c;
	SerialProtocolHandler SerialHandler("/dev/serial0",921600,8,1,'N',callback);
    SerialHandler.start_cv();
	//发送PID参数
	//SerialHandler.send(&user_send_data2,sizeof(send_data2_t));

	wmj::UVCCamera cap(UVC_CONFIG);
    cap.infoPrint();
    
    cv::Mat frame,hsv,binary;
    cv::Scalar green_low=cv::Scalar(50,100,115);
    cv::Scalar green_high=cv::Scalar(90,255,255);
    int height,width=0;
	AsyncVideoWriter processed_image_writer;
	//AsyncVideoWriter unprocessed_image_writer;
	DART_STATUS dart_status = DART_STATUS::DART_STATUS_NOT_READY;
    	//DART_STATUS dart_status = DART_STATUS::DART_STATUS_READY;

    std::cout<<"############## inited ###############"<<std::endl;
    sleep(1);
    cap >> frame;
    if(frame.empty())
    {
		std::cout <<"cap >> frame failed,frame is empty"<<std::endl;
		return -1;
	}
    height = frame.rows;
    width = frame.cols;

    double cx = width / 2;
    double cy = height / 2;
    
	std::string current_path = __FILE__; //filestream中当前文件路径的表述方式
	std::string src_path = std::filesystem::path(current_path).parent_path().string();
	//std::cout <<src_path<<std::endl;
	std::string string_command = src_path + "/mkdir_video.sh";
	std::cout <<string_command<<std::endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(200));
	char* command = const_cast<char*>(string_command.c_str());
	system(command);
	std::string parent_path = std::filesystem::path(current_path).parent_path().parent_path().string();

	std::string yaml_path = src_path + "/config.yaml";
	cv::FileStorage config(yaml_path, cv::FileStorage::READ);
	if(!config.isOpened())
	{
		std::cout << "Failed to open yaml at " << yaml_path << std::endl;
	}

	config["fx"] >> fx;
	config["fy"] >> fy;
	config["duration"] >> duration;
	config["debug"] >> debug;
	config["show"] >> show;
	
	//std::string video_path = parent_path + "/video/record_" +std::to_string((int)now());
	std::string video_path = parent_path + "/video/record_" +argv[1];
    processed_image_writer.open(video_path + "_processed.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 50, cv::Size(width,height));
	//unprocessed_image_writer.open(video_path + "_unprocessed.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 50, cv::Size(width,height));

	// while(dart_status == DART_STATUS::DART_STATUS_NOT_READY)
	// {
	// 	cap >> frame;
	// 	if(frame.empty())
	// 	{
	// 		std::cout<<"Could not load image..."<<std::endl;
    //         break;
	// 	}
	// 	bool can_change_status = canChangeStatus(frame);
	// 	if(can_change_status) dart_status = DART_STATUS::DART_STATUS_READY;
	// 	else std::this_thread::sleep_for(std::chrono::milliseconds(50));//设置一定的等待时间,避免忙等待
	// }
	//判断是开机脚本启动还是手动启动，开机脚本启动根据接收数据的状态来判断是否开始运行
	while(start_mode == true && preceive_data->dart_status==0) 
	{
		//睡眠10ms
		usleep(10*1000);
	}
	double timer_start=now();

	int image_count=0;
	double t10s=now();

	while(now()-t10s<=duration && cv::waitKey(1)!='q')
	{
		cap >> frame;
		if(frame.empty())
		{
			std::cout<<"Could not load image..."<<std::endl;
            break;
		}
		
		cv::Mat image=frame.clone();
		//unprocessed_image_writer << std::move(frame);
		cv::cvtColor(image,hsv,cv::COLOR_BGR2HSV);
		cv::inRange(hsv,green_low,green_high,binary);
		
		//Gaussian blur
		cv::GaussianBlur(binary,binary,cv::Size(7,7),1,1);
		
		if(show)
		{
			cv::imshow("origin",image);
			cv::imshow("mask",binary);	
		}
		
		std::vector<std::vector<cv::Point>> contours;
		cv::findContours(binary,contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_NONE);
		
		bool is_locked=false;
		cv::Point2f center=cv::Point2f(0,0);
		int best_index=0;
		double max_area=0;
		int contour_num=contours.size();
		
		for(int i=0;i<contour_num;i++)
		{
			if(debug) std::cout <<"contour info:"<<std::endl;
			std::vector<cv::Point> contour=contours[i];
			double contour_area=cv::contourArea(contour);
			if(debug) std::cout <<"\tcontour area: "<<contour_area<<std::endl;
			
			//hull
			std::vector<cv::Point> hull;
			cv::convexHull(contour,hull);
			double hull_area=cv::contourArea(hull);
			double convex_ratio=hull_area/contour_area;
			if(debug) std::cout <<"\tconvex ratio: "<<convex_ratio<<std::endl;
			if(hull_area==0 || convex_ratio<0.9) continue;
			
			//circularity
			//double perimeter=cv::arcLength(contour,true);
			//double circularity=4*CV_PI*contour_area/(perimeter*perimeter);
			//if(debug) std::cout <<"\tcircularity: "<<circularity<<std::endl;
			//if(circularity<=0.8) continue;
			
			
			//regular ratio
			cv::RotatedRect rect=cv::minAreaRect(contour);
			double regular_ratio=rect.size.width/rect.size.height;
			if(debug) std::cout <<"\tregular ratio: "<<regular_ratio<<std::endl;
			if(regular_ratio<0.8 || regular_ratio>1.25) continue;
			
			
			if(contour_area > max_area)
			{
				max_area=contour_area;
				best_index=i;
				is_locked=true;
			}
		}
		
		std::cout <<"contour selection finished."<<std::endl;
		if(!is_locked)
		{
						//发送数据
			user_send_data1.data_valid = 0u;
			user_send_data1.door_status = 1u;
			SerialHandler.send(&user_send_data1,sizeof(send_data1_t));

			if(debug) std::cout <<"not locked."<<std::endl;
			processed_image_writer << std::move(image);
			double fps=1/(now()-timer_start);
			std::cout <<"fps: "<<fps<<std::endl;
			std::cout <<"--------------------------------"<<std::endl;
			timer_start=now();
			continue;
		}
		
		if(debug) std::cout <<"no less than 1 contours found."<<std::endl;
		std::vector<cv::Point> final_contour=contours[best_index];
		if(final_contour.size()<5)
		{
			int points=final_contour.size();
			for(const cv::Point &point : final_contour)
			{
				center+=(cv::Point2f)point;
			}
			center=center/points;
		}
		else
		{
			cv::RotatedRect ellipse=cv::fitEllipseAMS(final_contour);
			center=ellipse.center;
		}
		if(debug) std::cout <<"center: "<<center<<std::endl;

		double x_3d=(center.x-cx)/fx;
		double y_3d=(center.y-cy)/fy;

		//发送数据
		user_send_data1.center_x = (center.x - cx);
		user_send_data1.center_y = (center.y - cy);
		user_send_data1.x_3d = x_3d;
		user_send_data1.y_3d = y_3d;
		user_send_data1.data_valid = 1u;
		user_send_data1.door_status = 1u;
		SerialHandler.send(&user_send_data1,sizeof(send_data1_t));
		
		if(debug) printf("center ray: (%lf, %lf, %d)",x_3d,y_3d,1);
		
		cv::putText(image,"Target locked",cv::Point(20,40),cv::FONT_HERSHEY_COMPLEX,1,cv::Scalar(0,255,255),1);
		cv::circle(image,center,10,cv::Scalar(255,0,0),3);
		cv::line(image,center-cv::Point2f(10,0),center+cv::Point2f(0,10),cv::Scalar(255,0,0),2);
		cv::line(image,center-cv::Point2f(0,10),center+cv::Point2f(10,0),cv::Scalar(255,0,0),2);
		
		processed_image_writer << std::move(image);
		
		double fps=1/(now()-timer_start);
		std::cout <<"fps: "<<fps<<std::endl;
		std::cout <<"--------------------------------"<<std::endl;
		timer_start=now();
		
	}
	cap.is_running=false;
	std::cout <<"-----------finish------------"<<std::endl;

	processed_image_writer.release();
	SerialHandler.stop();
}
